/**
* @file AIManager.cpp
* @author Andreas Karlsson
* @version 2010-01-05
*
* <code>AIManager</code> is a class that does pathfinding, updates formations and aiming for
* units in a GameState.
**/

#include "AIManager.h"
#include "../engine/Gamestate.h"


namespace Putin{

/**
* Constructs a new AIManager.
*
* @param g Gamestate with map and units.
*/
AIManager::AIManager(Gamestate* g)
{
	mGame = g;
	mMap = g->getMap();
	mPather = new MicroPather(mMap);
}

AIManager::~AIManager(void)
{
}

/**
* Updates the AI simulation.
*
* @param elapsedTime Time elapsed since the last update.
*/
bool AIManager::update(double elapsedTime) {
	bool formationStatus = updateFormations(elapsedTime);
	bool targetingStatus = updateTargeting(elapsedTime);
	return formationStatus && targetingStatus;
}

/**
* Updates unit targeting and aiming.
*
* @param elapsedTime Time elapsed since the last update.
*/
bool AIManager::updateTargeting(double elapsedTime) {
	
	// This is horribly ugly and scales extremely bad, *temporary* !
	// TODO: Use Quadtree or something similar to find possible targets more efficently.
	for each(std::pair<int,Unit*> up in mGame->mUnits)
	{
		Unit* unit = up.second;
		int id = unit->getID();
		Player* player = unit->getPlayer();

	
		bool foundTarget = false;
		for each(std::pair<int,Unit*> tup in mGame->mUnits)
		{
			Unit* tunit = tup.second;
			int tid = tunit->getID();
			Player* tplayer = tunit->getPlayer();
			
			if(player != tplayer) // found enemy unit
			{
				float range = (tunit->getPosition() - unit->getPosition()).length();
				 // in range and no current target, or found a closer target.
				if(range <= unit->getShootingRange() && (!foundTarget || range < (unit->getShootingTarget() - unit->getPosition()).length()))
				{
					// calculate aiming position, aim ahead if target is moving.
					Vec3f targetPosition = tunit->getPosition();
					if(tunit->getState() == Unit::MOVING)
						targetPosition = targetPosition + (tunit->getFacingDirection().normalized() * 15.0f);
					
					Vec3f aimVector = (targetPosition - unit->getPosition()).normalized();
					Vec3f tankFacing = unit->getFacingDirection().normalized();
					
					// Set turret aiming
					float aimDirection = atan2(tankFacing.y, tankFacing.x) - atan2f(aimVector.y, aimVector.x);
					unit->setChildFacingDirection(Vec3f(0,0,aimDirection));
					
					unit->shootAt(targetPosition);
					foundTarget = true;
					
				}
			}
		}
		// set turret to face front if no target has been found.
		if(!foundTarget)
		{
			unit->setChildFacingDirection(Vec3f(0,0,0));
			unit->stopShooting();
		}

	}
	return true;
}

/**
* Updates formations and pathfinding.
* Units are considered to be in formation if they have the exact same move target position.
* This happens if a user selects a group and clicks on the ground for example.
*
* @param elapsedTime Time elapsed since the last update.
*/
bool AIManager::updateFormations(double elapsedTime) {

	// update pathways, and do pathfinding
	for each(std::pair<int,Unit*> up in mGame->mUnits)
	{
		
		Unit* unit = up.second;
		int id = unit->getID();

		// only process moving units
		if(unit->getState() == unit->MOVING)
		{
			bool foundFormation = false;

			bool notInFormation = (mInFormation.find(id) == mInFormation.end());
			bool inWrongFormation = false;
			if(!notInFormation)
			{
				Vec3f dest = mInFormation.find(id)->second->getDestination();
				Vec3f movetarget = unit->getMoveTarget();
				inWrongFormation = (dest != movetarget);
			}
			// unit in no formation yet, or is in the wrong one
			if(notInFormation || inWrongFormation)
			{
				
				// look for nearby units with formation to target
				for each( std::pair<int,Formation*> up in mInFormation)
				{
					if(up.second->getDestination() == unit->getMoveTarget())
					{
						up.second->reserveSlot(id, unit->getPosition());
						unit->setNextWaypoint(up.second->getCurrentLeg(id));
						mInFormation[id] = up.second;
						foundFormation = true;
						break;
					}
				}

				// else create new formation
				if(!foundFormation)
				{
					std::vector<Vec3f*> path = getPathBetween(&unit->getPosition(), &unit->getMoveTarget());
					if(path.empty()) // couldn't find a path, set waypoint to current pos and idle
					{
						unit->setNextWaypoint(unit->getPosition());
						unit->moveTo(unit->getPosition());
						unit->setState(unit->IDLE);
						continue;
					}
					else
					{
						Formation* form = new Formation(path);
						form->reserveSlot(id, unit->getPosition()); // reserve nearest availible slot
						unit->setNextWaypoint(form->getCurrentLeg(id));
						mInFormation[id] = form;
					}
				}
			}
			
			Formation* formation = mInFormation[id];

		
			// reached current leg
			float distanceToGoal = (formation->getCurrentLeg(id) - unit->getPosition()).length();
			if(distanceToGoal < 40.0f || distanceToGoal < unit->mMaxSpeed)
			{
				// get next leg
				if(formation->nextLeg(id))
				{
					unit->setNextWaypoint(formation->getCurrentLeg(id));
				}
				else
				{
					//formation->releaseSlot(id);
					//mInFormation.erase(id);
					unit->setState(unit->IDLE);
				}
			}
		}
	}
	return true;
}

/**
* Get an efficient path between two map coordinates.
*
* @param x1 Starting position x
* @param y1 Starting position y
* @param x2 Destination x
* @param y2 Destination y
* @return Clean path consisting of map integer coordinates.
*/
vector<Vec2i*> AIManager::getPathBetween(int x1, int y1, int x2, int y2)
{
	vector<Vec2i*> vectorPath;

	// if destination isn't accessible, return empty result
	if(mMap->isObstacle(x2,y2) || mMap->getClearing(x2,y2) < 4)
	{
		return vectorPath;
	}

	std::vector< void* > path;
	float totalCost;
	// convert from coordinates to unique integer index (used as id for micropather)
	int p1 = mMap->posToIndex(x1, y1);
	int p2 = mMap->posToIndex(x2, y2);

	// try to find a path
	int result = mPather->Solve((void*)p1, (void*)p2, &path, &totalCost );
	
	// if we couldn't solve, return result with only starting position
	if (result != MicroPather::SOLVED)
	{
		vectorPath.push_back(new Vec2i(x1,y1));
		return vectorPath;
	}

	// convert to vector of Vec2i instead of index ints, and skip useless waypoints
	// useless waypoints are: points forcing the unit move diagonally/horizontally/vertical because of the grid structure
	// even though it can move straight in the world
	// these points are removed by deleting points not close to a possible turn
	for(unsigned int i=0; i<path.size(); i++) {
		int p = (int)path.at(i);
		Vec2i pos = mMap->indexToPos(p);
		if(i == 0 && path.size() > 1) // skip first if we have many
			continue;
		if(i == 0 || i == path.size() - 1 || mMap->nearPossibleTurn(&pos))
			vectorPath.push_back(new Vec2i(pos));
	}

	return vectorPath;
}

/**
* Get an efficient path between two map coordinates.
*
* @param p1 Starting position
* @param p2 Destination
* @return Clean path consisting of map integer coordinates.
*/
vector<Vec2i*> AIManager::getPathBetween(Vec2i* p1, Vec2i* p2)
{
	return getPathBetween(p1->x, p1->y, p2->x, p2->y);
}

/**
* Get an efficient path between two world coordinates.
*
* @param p1 Starting position
* @param p2 Destination
* @return Clean path consisting of world float coordinates. Only x and y are used, z is not used.
*/
vector<Vec3f*> AIManager::getPathBetween(Vec3f* p1, Vec3f* p2)
{
	Vec2i* p1i = worldToMap(p1);
	Vec2i* p2i = worldToMap(p2);

	vector<Vec2i*> path = getPathBetween(p1i, p2i);
	vector<Vec3f*> worldPath;

	for each( Vec2i* p in path)
	{
		worldPath.push_back(mapToWorld(p));
	}

	// add final destination on the final square
	if(!worldPath.empty())
	{
		worldPath.pop_back();
		worldPath.push_back(new Vec3f(*p2));
	}

	return worldPath;
}

/**
* Convert world coordinate to map integer coordinate.
* TODO: This should be moved elsewhere (not in AI)
*
* @param p World float coordinate, only x and y are used.
* @return Map integer coordinate.
*/
Vec2i* AIManager::worldToMap(Vec3f* p)
{
	return new Vec2i((int) p->x/16.0f, (int) p->y/16.0f);
}

/**
* Convert map integer coordinate to world float coordinate.
* TODO: This should be moved elsewhere (not in AI)
*
* @param p Map integer coordinate.
* @return World float coordinate, in the center of correspoding map grid, z is not used.
*/
Vec3f* AIManager::mapToWorld(Vec2i* p)
{
	return new Vec3f(((float) p->x)*16.0f + 8.0f, ((float) p->y)*16.0f + 8.0f, 40.0f);
}

} // End namespace putin